#include "cmodelmatrix3d.h"

/**
 * CModelMatrix3D - creates identity 4x4 matrix
 */
template <typename T>
CModelMatrix3D<T>::CModelMatrix3D() {
  this->init(this->M4X4_SIZE, this->M4X4_SIZE);
}
//---------------------------------------------------------------------------

/**
 * CModelMatrix3D - creates 4x4 matrix with given elements
 * @param items
 */
template <typename T>
CModelMatrix3D<T>::CModelMatrix3D(T items[]) {
  this->init(this->M4X4_SIZE, this->M4X4_SIZE, items);
}
//---------------------------------------------------------------------------

/**
 * CModelMatrix3D - copy constructor (does deep copy)
 * @param m
 */
template <typename T>
CModelMatrix3D<T>::CModelMatrix3D(const CModelMatrix3D<T> &m) {
  this->copyFrom(m);
}
//---------------------------------------------------------------------------

/**
 * CModelMatrix3D - copy constructor (does deep copy)
 * @param m
 */
template <typename T>
CModelMatrix3D<T>::CModelMatrix3D(const CMatrixBase<T> &m) {

  // If given matrix is not 4x4, exception
  if (m.getNumOfRows() != this->M4X4_SIZE ||
      m.getNumOfCols() != this->M4X4_SIZE) {
    throw CInvalidSizeException(getClassName(), "CModelMatrix3D");
  }
  this->copyFrom(m);
}
//---------------------------------------------------------------------------

/**
 * ~CModelMatrix3D - destructor
 */
template <typename T>
CModelMatrix3D<T>::~CModelMatrix3D() {
  this->free();
}
//---------------------------------------------------------------------------

/**
 * operator= - matrix assignment (does deep copy)
 * @param m
 * @return this
 */
template <typename T>
CModelMatrix3D<T>& CModelMatrix3D<T>::operator = (const CModelMatrix3D<T> &m) {
  this->free();
  this->copyFrom(m);
  return *this;
}
//---------------------------------------------------------------------------

/**
 * operator+ - adds two given 4x4 matrices
 * @param m
 * @return sum
 */
template <typename T>
CModelMatrix3D<T> CModelMatrix3D<T>::operator + (
  const CModelMatrix3D<T> &m) const {
  return CModelMatrix3D<T>(*this) += m;
}
//---------------------------------------------------------------------------

/**
 * operator- - subtracts two given 4x4 matrices
 * @param m
 * @return difference
 */
template <typename T>
CModelMatrix3D<T> CModelMatrix3D<T>::operator - (
  const CModelMatrix3D<T> &m) const {
  return CModelMatrix3D<T>(*this) -= m;
}
//---------------------------------------------------------------------------

/**
 * operator* - multiplies two given 4x4 matrices
 * @param m
 * @return product
 */
template <typename T>
CModelMatrix3D<T> CModelMatrix3D<T>::operator * (
  const CModelMatrix3D<T> &m) const {
  return CModelMatrix3D<T>(*this) *= m;
}
//---------------------------------------------------------------------------

/**
 * operator* - multiplies given 4x4 matrix by given number
 * @param num
 * @return result
 */
template <typename T>
CModelMatrix3D<T> CModelMatrix3D<T>::operator * (T num) const {
  return CModelMatrix3D<T>(*this) *= num;
}
//---------------------------------------------------------------------------

/**
 * operator/ - divides given 4x4 matrix by given number
 * @param num
 * @return result
 */
template <typename T>
CModelMatrix3D<T> CModelMatrix3D<T>::operator / (T num) const {
  return CModelMatrix3D<T>(*this) /= num;
}
//---------------------------------------------------------------------------

/**
 * getNumOfRows - returns number of 4x4 matrix rows
 * @return always four
 */
template <typename T>
TIndex CModelMatrix3D<T>::getNumOfRows() const {
  return this->M4X4_SIZE;
}
//---------------------------------------------------------------------------

/**
 * getNumOfCols - returns number of 4x4 matrix columns
 * @return always four
 */
template <typename T>
TIndex CModelMatrix3D<T>::getNumOfCols() const {
  return this->M4X4_SIZE;
}
//---------------------------------------------------------------------------

/**
 * translate - translates 4x4 matrix
 * @param x translation in x-axis
 * @param y translation in y-axis
 * @param z translation in z-axis
 */
template <typename T>
void CModelMatrix3D<T>::translate(T x, T y, T z) {

  // Temporary translation matrix
  CModelMatrix3D<T> tmp;
  tmp.clearToIdentity();
  tmp.set(3, 0, x);
  tmp.set(3, 1, y);
  tmp.set(3, 2, z);

  // Multiply temporary translation matrix by this matrix
  *this = tmp * (*this);
}
//---------------------------------------------------------------------------

/**
 * translate - translates 4x4 matrix
 * @param v 3D translation vector
 */
template <typename T>
void CModelMatrix3D<T>::translate(const CVector3D<T> &v) {
  translate(v.getX(), v.getY(), v.getZ());
}
//---------------------------------------------------------------------------

/**
 * rescale - scales 4x4 matrix
 * @param x scale factor for x-axis
 * @param y scale factor for y-axis
 * @param z scale factor for z-axis
 */
template <typename T>
void CModelMatrix3D<T>::rescale(T x, T y, T z) {

  // Temporary scale matrix
  CModelMatrix3D<T> tmp;
  tmp.clearToIdentity();
  tmp.set(0, 0, x);
  tmp.set(1, 1, y);
  tmp.set(2, 2, z);

  // Multiply temporary scale matrix by this matrix
  *this = tmp * (*this);
}
//---------------------------------------------------------------------------

/**
 * rescale -  scales 4x4 matrix
 * @param v 3D vector of scale factors
 */
template <typename T>
void CModelMatrix3D<T>::rescale(const CVector3D<T> &v) {
  rescale(v.getX(), v.getY(), v.getZ());
}
//---------------------------------------------------------------------------

/**
 * rotate - vynasobi matici rotacni matici pro rotaci o danych parametrech
 * @param angle angle (positive value for counter-clockwise rotation)
 * @param x rotation control for x-axis
 * @param y rotation control for y-axis
 * @param z rotation control for z-axis
 */
template <typename T>
void CModelMatrix3D<T>::rotate(T angle, T x, T y, T z) {

  // Pom. prm.
  T s(sinf(CMath<T>::degToRad(angle))), c(cosf(CMath<T>::degToRad(angle)));
  CVector3D<T> v = CVector3D<T>(-x, -y, -z);
  v.normalize();

  // Temporary rotation matrix
  CModelMatrix3D<T> r;
  r.clearToIdentity();
  r.set(0, 0, pow(v.getX(), 2) + c * pow(v.getZ(), 2) + c * pow(v.getY(), 2));
  r.set(0, 1, v.getY() * v.getX() - v.getZ() * s - v.getX() * v.getY() * c);
  r.set(0, 2, v.getZ() * v.getX() + v.getY() * s - v.getZ() * v.getX() * c);
  r.set(1, 0, v.getY() * v.getX() + v.getZ() * s - v.getX() * v.getY() * c);
  r.set(1, 1, pow(v.getY(), 2) + c * pow(v.getZ(), 2) + c * pow(v.getX(), 2));
  r.set(1, 2, v.getZ() * v.getY() - v.getX() * s - v.getZ() * v.getY() * c);
  r.set(2, 0, v.getZ() * v.getX() - v.getY() * s - v.getZ() * v.getX() * c);
  r.set(2, 1, v.getZ() * v.getY() + v.getX() * s - v.getZ() * v.getY() * c);
  r.set(2, 2, pow(v.getZ(), 2) + c * pow(v.getY(), 2) + c * pow(v.getX(), 2));

  // Multiply temporary rotation matrix by this matrix
  *this = r * (*this);
}
//---------------------------------------------------------------------------

/**
 * mul - multiplies 4x4 matrix by vector
 * @param m matrix
 * @param v vector
 * @return result (vector)
 */
template <typename T>
CVector3D<T> CModelMatrix3D<T>::mul(const CModelMatrix3D<T> &m,
                                    const CVector3D<T> &v) {

  // Temporary vector (resized to size 4)
  CVector<T> tmp(CModelMatrix3D<T>::M4X4_SIZE);
  tmp.set(0, v.getX());
  tmp.set(1, v.getY());
  tmp.set(2, v.getZ());
  tmp.set(3, 0);

  // Multiply matrix by vector
  CVector<T> result(CMatrixBase<T>::mul(m, tmp));
  return CVector3D<T>(result.get(0), result.get(1), result.get(2));
}
//---------------------------------------------------------------------------

/**
 * mul - multiplies vector by vector 4x4 matrix
 * @param v vector
 * @param m matrix
 * @return result (vector)
 */
template <typename T>
CVector3D<T> CModelMatrix3D<T>::mul(const CVector3D<T> &v,
                                    const CModelMatrix3D<T> &m) {

  // Temporary vector (resized to size 4)
  CVector<T> tmp(CModelMatrix3D<T>::M4X4_SIZE);
  tmp.set(0, v.getX());
  tmp.set(1, v.getY());
  tmp.set(2, v.getZ());
  tmp.set(3, 0);

  // Multiply vector by matrix
  CVector<T> result(CMatrixBase<T>::mul(tmp, m));
  return CVector3D<T>(result.get(0), result.get(1), result.get(2));
}
//---------------------------------------------------------------------------

template class CModelMatrix3D<float>;
template class CModelMatrix3D<double>;
